#include "../globals/globals.h"
#include "ActionUnit.h"

///////////////////////////////////
template <typename... Arg> void report_screen_and_cout(color c, Arg... other) {
  const static int h = Brain.Screen.getStringHeight(" ");
  static int line_y = 0;
  screen_set_pen_color(c);
  screen_set_pen_font(fontType::mono30);
  Brain.Screen.printAt(0, line_y += h + 12, true, other...);
  printf(other...);
  printf("\n");
}

//马达接口冲突检查
void ActionUnit::motor_port_conflict_check() {
  //预检测
  bool conflict = false;
  int idx1[22] = {};
  for (auto pau : vec_ptr_all_instacnes)
    for (int k = 0; k < pau->vec_ptr_motors.size(); k++) //所有马达
      if (pau->is_indepent_component) {
        if (idx1[pau->vec_ptr_motors[k]->index() + 1] == 0) {
          idx1[pau->vec_ptr_motors[k]->index() + 1] =
              pau->vec_ptr_motors[k]->index() + 1;
        } else {
          conflict = true;
          goto report; //有冲突，退到最外面
        }
      }
report:
  if (conflict) {
    cout << endl << endl;
    int idx[22] = {};
    string side = "";
    for (auto pau : vec_ptr_all_instacnes)
      for (int k = 0; k < pau->vec_ptr_motors.size(); k++) //所有马达
        if (pau->is_indepent_component) {

          if (idx[pau->vec_ptr_motors[k]->index() + 1] == 0) {
            idx[pau->vec_ptr_motors[k]->index() + 1] =
                pau->vec_ptr_motors[k]->index() + 1;

            report_screen_and_cout(green, " %-10s %s%2d %s",
                                   pau->get_motor_name_with_side_suffix(k),
                                   "PORT:", pau->vec_ptr_motors[k]->index() + 1,
                                   "√ ");
          } else {
            cout << "error: \n";
            report_screen_and_cout(
                red, " %-10s %s%2d %s", pau->get_motor_name_with_side_suffix(k),
                "PORT:", pau->vec_ptr_motors[k]->index() + 1, "X conflicted");
          }
        }
    while_wait_timeout(!special_key.pressing(), 30000); //等待查看
    screen_clear(black);
  }
}

/////////////
int motor_install_detecting = true; //临时调试关闭马达连接检测
extern void controller_print_team_number_and_autoinfo();
void ActionUnit::motors_error_check_routine() {
  //临时不检查
  if (team_name == "74000")
    return;
  //预检测
  int cnt = 0;
  for (auto pau : vec_ptr_all_instacnes) //所有AU实例
  {
    //////////
    for (int k = 0; k < pau->vec_ptr_motors.size(); k++) //所有马达
      if (!pau->vec_ptr_motors[k]->installed())
        cnt++;
  }
  if (cnt > 2) {
    motor_install_detecting = false; //未连接马达超过2个，则取消连接检查
    ctrl_screen_display_err_info = true; //临时借用遥控屏幕显示
    ctrl.Screen.clearScreen();
    ctrl.Screen.setCursor(1, 1);
    ctrl.Screen.print("%d motors uninstalled", cnt);
    while_wait(!special_key.pressing());
    ctrl.Screen.clearScreen();
    controller_print_team_number_and_autoinfo();
    ctrl_screen_display_err_info = false;
  }

  //////////////////////////////////////常规检测
  int n = 0;
  while (1) {
    for (auto pau : vec_ptr_all_instacnes) { //所有AU实例
      // 1: 连接检查, 5次间隔，
      if (n % 5 == 0 && motor_install_detecting) {
        for (int k = 0; k < pau->vec_ptr_motors.size(); k++) //所有马达
          if (!pau->vec_ptr_motors[k]->installed()) {
            ctrl.rumble(rumble_motor_uninstalled);
            ctrl_screen_display_err_info = true; //临时借用遥控屏幕显示
            ctrl.Screen.clearScreen();
            ctrl.Screen.setCursor(1, 1);
            ctrl.Screen.print("%s", pau->get_name_c_str());
            ctrl.Screen.setCursor(2, 1);
            ctrl.Screen.print("motor uninstalled");
            ctrl.Screen.setCursor(3, 1);
            ctrl.Screen.print("port:%d  motor:%d",
                              pau->vec_ptr_motors[k]->index() + 1, k);
            cout << "error: " << pau->get_name() << " motor:" << k
                 << " port: " << pau->vec_ptr_motors[k]->index() + 1
                 << " uninstalled!!\n";
            timer t;
            while (t.time() < 1500) {
              if (special_key.pressing())
                motor_install_detecting = false; //震动期间取消震动
              wait(50);
            }
            ctrl.Screen.clearScreen();
            controller_print_team_number_and_autoinfo();
            ctrl_screen_display_err_info = false;
          }
      }

      // 2:脱轴检测
      if (pau->shaft_disengaged_detecting == true &&
          pau->get_motor_count() >= 2 &&
          !special_key.pressing()) //含有多马达,排除底盘left按键单独测试
      {
        double max_speed_abs = 0, min_current = 100, min_speed_abs = 100;
        int max_speed_k = 0, min_current_k = 0, max_current = 0;
        for (int k = 0; k < pau->vec_ptr_motors.size();
             k++) //所有AU实例所包含的马达
        {
          if (!pau->vec_ptr_motors[k]->installed())
            break; //有马达掉线，不再检测脱轴
          //寻找所有马达中的最大速度及其编号
          double speed_x =
              abs(pau->vec_ptr_motors[k]->velocity(percentUnits::pct));
          if (speed_x > max_speed_abs) {
            max_speed_abs = speed_x; //记录最大速度
            max_speed_k = k;         //保存马达编号
          }
          if (speed_x < min_speed_abs)
            min_speed_abs = speed_x; //记录最小速度

          //寻找最小电流及其编号
          double current_x = pau->vec_ptr_motors[k]->current(pct);
          if (current_x < min_current) {
            min_current = current_x; //记录最小电流
            min_current_k = k;       //保存马达编号
          }
          if (current_x > max_current)
            max_current = current_x; //记录最大电流
          // cout << pau->get_name() << " " << k << ":" << speed_x << "---"
          //      << current_x << endl;
        }
        // cout << max_speed_k << " " << min_current_k << endl;

        // 脱轴：同一个马达速度最大，电流最小
        if (max_speed_k == min_current_k &&
            (max_speed_abs - min_speed_abs) > 10 &&
            (max_current - min_current) > 5) {
          pau->shaft_disengaged_cnt++;
        } else
          pau->shaft_disengaged_cnt = 0;

        if (pau->shaft_disengaged_cnt >= 4) {
          ctrl_screen_display_err_info = true; //临时借用遥控屏幕显示
          ctrl.rumble(rumble_motor_shaft_disengaged);
          ctrl.Screen.clearScreen();
          ctrl.Screen.setCursor(1, 1);
          ctrl.Screen.print("%s", pau->get_name_c_str());
          ctrl.Screen.setCursor(2, 1);
          ctrl.Screen.print("shaft disengaged");
          ctrl.Screen.setCursor(3, 1);
          ctrl.Screen.print("motor-->%d", max_speed_k);
          cout << "error: " << pau->get_name() << " motor " << max_speed_k
               << "-->shaft disengaged" << endl;
          wait(1500);
          ctrl.Screen.clearScreen();
          controller_print_team_number_and_autoinfo();
          ctrl_screen_display_err_info = false;
        }
      } //脱轴检测
    }   //所有AU实例
    wait(100);
    n++;
  } //主循环
}